/*
 *  kalmanAdvanced.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2007-12-8.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 *  This algorithm is the sequential Kalman Filter algorithm from "Kalman Filtering", Chap. 7, by C.K. Chui and G. Chen.
 *  The below code has been optimized to this specific 2D application, and cannot be copied/pasted into any other model.
 *
 */

#include "displayOpenCV.h"
#include "kalmanAdvanced.h"

enum states {X1, X2, X3, X4, X5, X6, X7, X8, X9, X10, X11, X12, X13, X14, X15, X16, states_enum_sentinel};
enum controls {U1, U2, U3, controls_enum_sentinel};
enum outputs {Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, outputs_enum_sentinel};


//#define N states_enum_sentinel                     /* number of first order equations*/
//#define L controls_enum_sentinel                     /* number of control inputs */
//#define M outputs_enum_sentinel                     /* number of measurment outputs */
//#define Nz (cvmGet( table.actuator_1, 0, 0 )*cvmGet( table.actuator_2, 0, 1 )-cvmGet( table.actuator_2, 0, 0 )*cvmGet( table.actuator_1, 0, 1 )) //x1*y2-x2*y1

extern double delT;
extern SystemDims table;
extern double kt;
extern double g;

#define R table.ballDia


void dS( CvMat *S, CvMat **k, double x[], CvMat *u, CvMat *Q );
double dx(double t, double x[], CvMat *u, int i );
//void runge4(double t, double x[], double u[], double step); /* Runge-Kutta function */
void fillAb( CvMat **Ab, double x[], CvMat *u );
void fillC( CvMat **C, CvMat **Czkm, CvMat *x_hat);

void kalmanSequential( CvMat **x_hatR, CvMat *y_measrdR, CvMat *u, CvKalman *kalman_dscrt ) {
    CvMat *Kk, PkmCol;
    
    CvMat *A_dscrt, *B_dscrt, *C, *Pkm, *R_kalman;
    
    A_dscrt=kalman_dscrt->transition_matrix;
    B_dscrt=kalman_dscrt->control_matrix;
    C=kalman_dscrt->measurement_matrix;
    Pkm=kalman_dscrt->error_cov_pre;
    R_kalman=kalman_dscrt->measurement_noise_cov;
    
    
    Kk=cvCreateMat( 4, 1, CV_32F);
    double c, r, Pkmii, x, y;
    
    
    for (int i=0; i<2; i++){
        // Prepare variables
        c=cvmGet( C, i, i*2 );
        r=cvmGet( R_kalman, i, i );
        x=cvmGet( *x_hatR, i*2, 0 );
        y=cvmGet( y_measrdR, i, 0 );
        Pkmii=cvmGet( Pkm, i*2, i*2 );
        cvGetCol( Pkm, &PkmCol, i*2);
        
        // Compute Kalman gain
        cvScale(&PkmCol, Kk, 1.0/(c*Pkmii+r/c));
        
        // Correct prediction values
        cvScaleAdd( Kk, cvScalar(y-c*x), *x_hatR, *x_hatR );
    }
    
    return;
}

void kalmanCD( CvMat **x_hat, CvMat *y_measrdR, CvMat *u, CvKalman **kalman_system ){
    
    CvMat *Pkm, *Q_kal, *R_kal;
    
    Pkm=(*kalman_system)->error_cov_pre;
    Q_kal=(*kalman_system)->process_noise_cov;
    R_kal=(*kalman_system)->measurement_noise_cov;
    
//    printf("Entering continuous prediction...\n");
    //===================
    //Continuous prediction
    //===================
    
    CvMat *S, *dSdt, *Sk;
    
    S=cvCreateMat( 16, 16, CV_32F );
    cvCopy((*kalman_system)->error_cov_post, S);
//    printCvArr(S);
    dSdt=cvCloneMat( S );
    Sk=cvCloneMat( S );
    
    int i, N=(*x_hat)->rows;
    double step=delT, h=step/2.0,                      // the midpoint
            xt0[N], xt1[N], xt2[N], xt3[N],            // temporary storage arrays
            k1[N], k2[N], k3[N], k4[N];      // for Runge-Kutta
    double t=0;
    
    
//    printf("Integrating x...\n");
    //Integrate dx
    for (i=0;i<N;i++) xt0[i]=cvmGet(*x_hat, i, 0);
    for (i=0;i<N;i++) xt1[i]=cvmGet(*x_hat, i, 0)+0.5*(k1[i]=step*dx(t, xt0, u, i));
    for (i=0;i<N;i++) xt2[i]=cvmGet(*x_hat, i, 0)+0.5*(k2[i]=step*dx(t+h, xt1, u, i));
    for (i=0;i<N;i++) xt3[i]=cvmGet(*x_hat, i, 0)+    (k3[i]=step*dx(t+h, xt2, u, i));
    for (i=0;i<N;i++) k4[i]=                                 step*dx(t+step, xt3, u, i);
    
    for (i=0;i<N;i++) cvmSet(*x_hat, i, 0, cvmGet(*x_hat, i, 0)+(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6.0);
    
//    printCvArr(*x_hat);
    
//    printf("Integrating S...\n");
    //Integrate dS
    CvMat *tmpS, *St2, *St3, *sk1, *sk2, *sk3, *sk4;
    
    tmpS=cvCloneMat( S );
    St2=cvCloneMat( S );
    St3=cvCloneMat( S );
    sk1=cvCloneMat( S );
    sk2=cvCloneMat( S );
    sk3=cvCloneMat( S );
    sk4=cvCloneMat( S );
    
//    printf("Integrating S1...\n");
//    for (int j=0; j<16; j++){
//        printf("%f, ", xt3[j]);
//    }
    
    dS(S, &dSdt, xt0, u, Q_kal);
//    printf("Done integrating S1...\n");
    cvScale( dSdt, sk1, step);
    cvScaleAdd(sk1, cvScalar(0.5), S, tmpS);
    
//    printf("Integrating S2...\n");
    dS(tmpS, &dSdt, xt1, u, Q_kal);
    cvScale( dSdt, sk2, step );
    cvScaleAdd(sk2, cvScalar(0.5), S, St2);
    
//    printf("Integrating S3...\n");
    dS(St2, &dSdt, xt2, u, Q_kal);
    cvScale( dSdt, sk3, step);
    cvAdd( sk3, S, St3 );
    
//    printf("Integrating S4...\n");
    dS(St3, &dSdt, xt3, u, Q_kal);
    cvScale( dSdt, sk4, step);
    
//    printf("Assembling integration steps...\n");
    cvScaleAdd( sk2, cvScalar(2), sk1, tmpS );
    cvScaleAdd( sk3, cvScalar(2), tmpS, tmpS );
    cvAdd( sk4, tmpS, tmpS );
    cvScaleAdd( tmpS, cvScalar(1/6.0), S, Sk );
    
    // Local garbage collection
    cvReleaseMat( &tmpS );
    cvReleaseMat( &St2 );
    cvReleaseMat( &St3 );
    cvReleaseMat( &sk1 );
    cvReleaseMat( &sk2 );
    cvReleaseMat( &sk3 );
    cvReleaseMat( &sk4 );
    cvReleaseMat( &dSdt );
    
//    printf("\n Sk- = \n");
//    printCvArr( Sk );
//    
//    printf("\n x_hat- = \n");
//    printCvArr( *x_hat );
    
//    printf("Entering discrete prediction...\n");
    //===================
    //Discrete correction
    //===================
    
    CvMat *C, *Czkm;
    CvMat *SkInv;
    CvMat *RInv, *CtRInv;
    CvMat *predErr, *kalCorr;
    
    Czkm=cvCloneMat(y_measrdR);
    C=cvCreateMat( 11, 16, CV_32F );
    SkInv=cvCloneMat( Sk );
    RInv=cvCloneMat( R_kal );
    kalCorr=cvCreateMat( 16, 11, CV_32F );
    CtRInv=cvCloneMat(kalCorr);
    predErr=cvCloneMat(y_measrdR);
    
//    printf("Forming C...\n");
    
    fillC( &C, &Czkm, *x_hat );
    
//    printf("\n x_hat = \n");
//    printCvArr( *x_hat );
//    
//    printf("\n y_measrdR = \n");
//    printCvArr( y_measrdR);
//    
//    printf("\n Czkm = \n");
//    printCvArr( Czkm);
//    
//    printf("\n C = \n");
//    printCvArr( C);
//    
//    
//    printf("\n Sk- = \n");
//    printCvArr( Sk );
//    
//    printf("\n Q_kal = \n");
//    printCvArr( Q_kal );
//    
//    printf("\n R_kal = \n");
//    printCvArr( R_kal );
    
//    printf("Inverting R...\n");
    cvInvert( R_kal, RInv, CV_SVD_SYM );
    
    cvGEMM(C, RInv, 1, 0, 0, CtRInv, CV_GEMM_A_T);
    cvGEMM(CtRInv, C, delT, Sk, 1, Sk, 0);
    
    cvInvert( Sk, SkInv, CV_SVD_SYM );
    
    cvScaleAdd( y_measrdR, cvScalar(-1), Czkm, predErr);
    
    cvGEMM( SkInv, CtRInv, delT, 0, 0, kalCorr, 0);
    cvGEMM( kalCorr, predErr, -1, *x_hat, 1, *x_hat, 0 );
    
//    printf("\n SkInv+ = \n");
//    printCvArr( SkInv );
//    
//    printf("\n CtRInv+ = \n");
//    printCvArr( CtRInv );
//    
//    printf("\n kalCorr = \n");
//    printCvArr( kalCorr );
//    printf("\n Sk-+ = \n");
//    printCvArr( Sk );
    
    //Add the transpose and take the average in order to ensure symmetry
    cvTranspose(Sk, S);
    cvAdd(Sk, S, Sk);
    cvScale( Sk, (*kalman_system)->error_cov_post, 0.5 );
    
//    printf("\n Sk+ = \n");
//    printCvArr( (*kalman_system)->error_cov_post );
//    
//    printf("\n x_hat+ = \n");
//    printCvArr( *x_hat );
//    printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
//    exit(1);
    
    // Garbage collection
    cvReleaseMat( &Czkm );
    cvReleaseMat( &Sk );
    cvReleaseMat( &SkInv );
    cvReleaseMat( &RInv );
    cvReleaseMat( &CtRInv );
    cvReleaseMat( &predErr );
    cvReleaseMat( &kalCorr );
    cvReleaseMat( &C );
    cvReleaseMat( &S );

    return;
}

void dS( CvMat *S, CvMat **dSdt, double x[], CvMat *u, CvMat *Q){
//      printf("Error 1\n");
    
    CvMat *Ab, *AbS, *AbSSAb, *SQ, *SQS;
    
    Ab=cvCloneMat( S );
    AbS=cvCloneMat( S );
    AbSSAb=cvCloneMat( S );
    SQ=cvCloneMat( S );
    SQS=cvCloneMat( S );
    
//    printf("\nx_predict = [");
//    for (int j=0; j<16; j++){
//        printf("%f, ", x[j]);
//    } printf("]\n");
    
    
    fillAb( &Ab, x, u );
    
    
    cvGEMM(Ab, S, -1, 0, 0, AbS, CV_GEMM_A_T);
    cvGEMM(S, Ab, -1, AbS, 1, AbSSAb);
    cvMatMul(S, Q, SQ);
    cvMatMul(SQ, S, SQS);
    cvSub(AbSSAb, SQS, *dSdt);
//    printf("Error 3\n");
    
//    printf("\ntmpS=\n");
//    printCvArr(S);
//    printf("\nAb=\n");
//    printCvArr(Ab);
//    printf("\nAbS=\n");
//    printCvArr(AbS);
//    printf("\nAbSSAb=\n");
//    printCvArr(AbSSAb);
//    printf("\nSQ=\n");
//    printCvArr(SQ);
//    printf("\nSQS=\n");
//    printCvArr(SQS);
//    printf("\ndSdt=\n");
//    printCvArr(*dSdt);
    
    // Garbage collection
    cvReleaseMat( &Ab );
    cvReleaseMat( &AbS );
    cvReleaseMat( &AbSSAb );
    cvReleaseMat( &SQ );
    cvReleaseMat( &SQS );
    
    return;
}


double dx(double t, double x[], CvMat *u, int i ){
      
    double Nx, Ny, Nz, Nn, Nx2, Ny2;
    double x1=cvmGet( table.actuator_1, 0, 0 );	double y1=cvmGet( table.actuator_1, 0, 1 );
    double x2=cvmGet( table.actuator_2, 0, 0 );	double y2=cvmGet( table.actuator_2, 0, 1 );
    double u0=cvmGet( u, 0, 0 ), u1=cvmGet( u, 1, 0 ), u2=cvmGet( u, 2, 0 );
    switch (i){
        case X1:
            return(x[X2]);                 // derivative of X1
            break;
        case X2:
            
            Nx=y1*(x[X7]-x[X5])-y2*(x[X6]-x[X5]);
            Ny=-x1*(x[X7]-x[X5])+x2*(x[X6]-x[X5]);
            Nz=x1*y2-x2*y1;
            Nx2=pow(Nx, 2);
            Ny2=pow(Ny, 2);
            Nn=sqrt(Nx2+Ny2+pow(Nz, 2));
            if (Nx==0 && Ny==0)
                return(0);
            else
                return(kt*g*Nx/sqrt(Nx2+Ny2+pow((Nx2+Ny2)/Nz, 2))*sqrt(Nx2+Ny2)/Nn);
            break;
        case X3:
            return(x[X4]);                 // derivative of X3
            break;
        case X4:
            Nx=y1*(x[X7]-x[X5])-y2*(x[X6]-x[X5]);
            Ny=-x1*(x[X7]-x[X5])+x2*(x[X6]-x[X5]);
            Nx2=pow(Nx, 2);
            Ny2=pow(Ny, 2);
            Nn=sqrt(Nx2+Ny2+pow(Nz, 2));
            if (Nx==0 && Ny==0)
                return(0);
            else
                return(kt*g*Ny/sqrt(Nx2+Ny2+pow((Nx2+Ny2)/Nz, 2))*sqrt(Nx2+Ny2)/Nn);
            break;
        case X5:
            double Nn2, Nz2, Nz3, Nz4;
            double zb;//, omegax, omegay;
            double psi, theta, ctheta, ctheta2;
            
            Nx=y1*(x[X7]-x[X5])-y2*(x[X6]-x[X5]);
            Ny=-x1*(x[X7]-x[X5])+x2*(x[X6]-x[X5]);
            Nn2=Nx2+Ny2+pow(Nz, 2);
            Nn=sqrt(Nn2);
            Nz2=pow(Nz, 2);
            Nz3=Nz2*Nz;
            Nz4=Nz2*Nz2;
            
            zb=-(Nx*x[X1]+Ny*x[X3])/Nz;
            
            psi=asin(-Ny/Nn);
            theta=acos(Nz/(Nn*cos(psi)));
            ctheta=cos(theta);
            ctheta2=pow(ctheta, 2);
//            omegax=((ctheta*Nz*(u1*(x2-x[X1])+u2*(-x1+x[X1])))/Nn+(ctheta*Nx*((u1*(-Nx*x[X1]-Ny*x[X3]))/Nz-(u2*(-Nx*x[X1]-Ny*x[X3]))/Nz+u2*x[6]-u1*x[7]))/Nn)/((-(Ny2/Nn2)+Nz2/(ctheta2*Nn2))*((Nz*(x[X1]*(y2-y1)+x2*(y1-x[X3])+x1*(-y2+x[X3])))/Nn-(Ny*(x2*(-((-Nx*x[X1]-Ny*x[X3])/Nz)+x[6])+x1*((-Nx*x[X1]-Ny*x[X3])/Nz-x[7])+x[X1]*(-x[6]+x[7])))/Nn+(Nx*(y2*(-((-Nx*x[X1]-Ny*x[X3])/Nz)+x[6])+y1*((-Nx*x[X1]-Ny*x[X3])/Nz-x[7])+x[X3]*(-x[6]+x[7])))/Nn));
//            omegay=(ctheta*(-((Nx*Ny*(u1*(x2-x[X1])+u2*(-x1+x[X1])))/Nn2)+(Nz2*(u1*(y2-x[X3])+u2*(-y1+x[X3])))/(ctheta2*Nn2))-(ctheta*Ny*Nz*(-((u1*(-Nx*x[X1]-Ny*x[X3]))/Nz)+(u2*(-Nx*x[X1]-Ny*x[X3]))/Nz-u2*x[6]+u1*x[7]))/Nn2)/((-(Ny2/Nn2)+Nz2/(ctheta2*Nn2))*((Nz*(x[X1]*(y2-y1)+x2*(y1-x[X3])+x1*(-y2+x[X3])))/Nn-(Ny*(x2*(-((-Nx*x[X1]-Ny*x[X3])/Nz)+x[6])+x1*((-Nx*x[X1]-Ny*x[X3])/Nz-x[7])+x[X1]*(-x[6]+x[7])))/Nn+(Nx*(y2*(-((-Nx*x[X1]-Ny*x[X3])/Nz)+x[6])+y1*((-Nx*x[X1]-Ny*x[X3])/Nz-x[7])+x[X3]*(-x[6]+x[7])))/Nn));
            
//            return(-1/(ctheta*Nn2*Nz2*Nz2)*(ctheta2*Ny2-Nz2)*(Nx2+Nz2)*(-Nz2*x[X3]*omegax+ctheta2*(Nx*Ny*x[X1]*omegax+Ny*Nz*zb*omegax+Nn*Nz*x[X1]*omegay-Nn*Nx*zb*omegay)));

            return(-(ctheta2*(Nx2+Nz2)*(Nz3*(-u2*x[X1]*y1+u1*x[X1]*y2+u2*x1*x[X3]-u1*x2*x[X3])+ctheta2*Nx2*Ny*(u2*x[X1]*x[X6]-u1*x[X1]*x[X7]-u2*x1*zb+u1*x2*zb)+Nz2*(ctheta2*Ny*(u2*x[X1]*x[X6]-u1*x[X1]*x[X7]-u2*x1*zb+u1*x2*zb)+Nx*(-u2*x[X3]*x[X6]+u1*x[X3]*x[X7]+u2*y1*zb-u1*y2*zb))))/(Nz4*(Nz2+Nz*(x[X1]*(y1-y2)+(-x1+x2)*x[X3])+Nx*(Nz+x[X1]*(y1-y2)+(-x1+x2)*x[X3])+Ny*(x[X1]*(-x[X6]+x[X7])+x2*(x[X6]-zb)+x1*(-x[X7]+zb))))+u0);
            break;
        case X6:
            return(u0+u2);                 // derivative of X1
            break;
        case X7:
            return(u1+u2);
            break;
        case X8:
            return(u2);
            break;
        case X9:
            return(-x[X2]/R*x[X11]);
            break;
        case X10:
            return(-x[X4]/R*x[X11]);
            break;
        case X11:
            return(x[X2]/R*x[X9]+x[X4]/R*x[X10]);
            break;
        case X12:
            return(-x[X2]/R*x[X14]);
            break;
        case X13:
            return(-x[X4]/R*x[X14]);
            break;
        case X14:
            return(x[X2]/R*x[X12]+x[X4]/R*x[X13]);
            break;
        case X15:
            return(0);
            break;
        case X16:
            return(0);
            break;
    }
    
    
//    return -1234567890; //LEAVE COMMENTED OUT IN ORDER TO UNDERSTAND BEHAVIOR OF LOOP WITHOUT RETURN;
}

/*
void runge4(double t, double x[], CvMat *u, double step) {
    int N=16;
    double h=step/2.0,                      // the midpoint 
            t1[N], t2[N], t3[N],            // temporary storage arrays 
            k1[N], k2[N], k3[N], k4[N];      // for Runge-Kutta 
    int i;
    
    for (i=0;i<N;i++) t1[i]=x[i]+0.5*(k1[i]=step*dx(t, x, u, i));
    for (i=0;i<N;i++) t2[i]=x[i]+0.5*(k2[i]=step*dx(t+h, t1, u, i));
    for (i=0;i<N;i++) t3[i]=x[i]+    (k3[i]=step*dx(t+h, t2, u, i));
    for (i=0;i<N;i++) k4[i]=                step*dx(t+step, t3, u, i);
    
    for (i=0;i<N;i++) x[i]+=(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6.0;
    
    return;
}
*/

void fillAb( CvMat **Ab, double x[], CvMat *u ) {
    
    CvMat *A, *b;
    
    A=cvCreateMat( 16, 16, CV_32F );
    cvZero(A);
    b=cvCloneMat( A );
    
    
    double b14, b15, b16, b34, b35, b36;
    double b40, b42, b44, b45, b46;
    double b81, b810, b93, b910, b101, b103, b108, b109;
    double b111, b1113, b123, b1213, b131, b133, b1311, b1312;
//    double den1, den2, num1, num2, num3;
    
    double Nx, Ny, Nz, Nx2, Ny2, Nz2, Nz3, Nz4, Nn, Nn3, Nx2Ny2, Nx2Nz2;
    double x2x1, y2y1, z2z1;
    double x1, x2, y1, y2, z0, z1, z2;
    double xb, yb, zb, vbx, vby;
    double xi, yi, zi, xk, yk, zk;
    double u1, u2;
    double psi, theta, ctheta, ctheta2;
    
    cvmSet( A, 0, 1, 1 );
    cvmSet( A, 2, 3, 1 );
    
    x1=cvmGet( table.actuator_1, 0, 0);
    y1=cvmGet( table.actuator_1, 0, 1);
    x2=cvmGet( table.actuator_2, 0, 0);
    y2=cvmGet( table.actuator_2, 0, 1);
    
    xb=x[X1];
    vbx=x[X2];
    yb=x[X3];
    vby=x[X4];
    z0=x[X5];
    z1=x[X6];
    z2=x[X7];
    
    xi=x[X9];
    yi=x[X10];
    zi=x[X11];
    xk=x[X12];
    yk=x[X13];
    zk=x[X14];
    
    u1=cvmGet( u, 0, 0 );
    u2=cvmGet( u, 1, 0 );
    
    Nx=y1*(z2-z0)-y2*(z1-z0);
    Ny=-x1*(z2-z0)+x2*(z1-z0);
    Nz=x1*y2-x2*y1;
    Nx2=pow(Nx, 2);
    Ny2=pow(Ny, 2);
    Nz2=pow(Nz, 2);
    Nz3=Nz2*Nz;
    Nz4=Nz3*Nz;
    Nx2Ny2=Nx2+Ny2;
    Nx2Nz2=Nx2+Nz2;
    Nn=sqrt(Nx2+Ny2+Nz2);
    Nn3=pow(Nn, 3);
    x2x1=x2-x1;
    y2y1=y2-y1;
    z2z1=z2-z1;
    
    zb=-(Nx*xb+Ny*yb)/Nz;
    
    psi=asin(-Ny/Nn);
    theta=acos(Nz/(Nn*cos(psi)));
    ctheta=cos(theta);
    ctheta2=pow(ctheta, 2);
    
//    double ktg=kt*g;
    if(Nx==0 && Ny==0){
        
        double den1=pow(Nz+xb*(y1-y2)+(-x1+x2)*yb, 2);
        
        b14=-kt*g*Nz2*(-y2y1)/Nn3;
        b15=-kt*g*Nz2*y2/Nn3;
        b16=kt*g*Nz2*y1/Nn3;
        
        b34=kt*g*Nz2*(-x2x1)/Nn3;
        b35=kt*g*Nz2*x2/Nn3;
        b36=-kt*g*Nz2*x1/Nn3;
        
        b40=(ctheta2*(Nz*(u2*y1-u1*y2)+(-u1+u2)*(x2*y1-x1*y2)*yb))/den1;
        b42=(ctheta2*(Nz*(-u2*x1+u1*x2)+(u1-u2)*xb*(x2*y1-x1*y2)))/den1;
        b44=(ctheta2*(-(Nz+xb*(y1-y2)+(-x1+x2)*yb)*(ctheta2*(x1-x2)*xb+(y1-y2)*yb)*(u2*z1-u1*z2)+(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)*((-y1+y2)*(Nz+xb*(y1-y2)+(-x1+x2)*yb)+(x1-x2)*(x2*z1-x1*z2+xb*(-z1+z2)))))/(Nz*den1);
        b45=(ctheta2*(-(Nz+xb*(y1-y2)+(-x1+x2)*yb)*(ctheta2*x2*xb+y2*yb)*(u2*z1-u1*z2)+(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)*(-y2*(Nz+xb*(y1-y2)+(-x1+x2)*yb)+x2*(x2*z1-x1*z2+xb*(-z1+z2)))))/(Nz*den1);
        b46=(ctheta2*((Nz+xb*(y1-y2)+(-x1+x2)*yb)*(ctheta2*x1*xb+y1*yb)*(u2*z1-u1*z2)+(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)*(y1*(Nz+xb*(y1-y2)+(-x1+x2)*yb)+x1*(-x2*z1+xb*(z1-z2)+x1*z2))))/(Nz*den1);
        
    }
    else{
        double den1=(Nx2Ny2+pow(Nx2Ny2, 2)/Nz2)*Nn3;
        double num1=kt*g*Nx2Ny2;
        
        b14=num1*(2*Nx*Ny*x2x1+(-Nx2+Ny2+Nz2)*y2y1)/den1;
        b15=num1*(-2*Nx*Ny*x2+(Nx2-Ny2-Nz2)*y2)/den1;
        b16=num1*(2*Nx*Ny*x1+(-Nx2+Ny2+Nz2)*y1)/den1;
        
        b34=num1*(2*Nx*Ny*x2x1+(-Nx2+Ny2+Nz2)*y2y1)/den1;
        b35=num1*((Nx2-Ny2+Nz2)*x2+2*Nx*Ny*y2)/den1;
        b36=num1*(-(Nx2-Ny2+Nz2)*x1-2*Nx*Ny*y1)/den1;
        
        
        
//        if(u1==0 && u2==0){ //LEAVE COMMENTED OUT FOR DEBUGGING PURPOSES
//            b40=0;
//            b42=0;
//            b44=0;
//            b45=0;
//            b46=0;                        
//        }
//        else{
            double den2=pow(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)), 2);
            double num2=(u2*xb*z1-u1*xb*z2-u2*x1*zb+u1*x2*zb);
            double num3=(-u2*yb*z1+u1*yb*z2+u2*y1*zb-u1*y2*zb);
            
            b40=(ctheta2*Nx2Nz2*((Nz3*(u2*y1-u1*y2)+ctheta2*Nx2*Ny*(-u2*z1+u1*z2)+ctheta2*Ny*Nz2*(-u2*z1+u1*z2))*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))+((Nx+Nz)*(-y2y1)+Ny*z2z1)*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))))/(Nz4*den2);
            b42=(ctheta2*Nx2Nz2*(Nz2*(-Nz*u2*x1+Nz*u1*x2+Nx*u2*z1-Nx*u1*z2)*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))+(Nx+Nz)*(-x2x1)*(Nz3*(u2*xb*y1-u1*xb*y2-u2*x1*yb+u1*x2*yb)+ctheta2*Nx2*Ny*(-u2*xb*z1+u1*xb*z2+u2*x1*zb-u1*x2*zb)+Nz2*(ctheta2*Ny*(-u2*xb*z1+u1*xb*z2+u2*x1*zb-u1*x2*zb)+Nx*(u2*yb*z1-u1*yb*z2-u2*y1*zb+u1*y2*zb)))))/(Nz4*den2);
            b44=(ctheta2*(Nx2Nz2*(ctheta2*(Nx2*(-x2x1)+Nz2*(-x2x1)+2*Nx*Ny*y2y1)*(-u2*xb*z1+u1*xb*z2+u2*x1*zb-u1*x2*zb)+Nz2*(-y2y1)*(Nx*num3))*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))+2*Nx*(-y2y1)*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))+Nx2Nz2*(y2y1*(Nz+xb*(-y2y1)+x2x1*yb)+(-x2x1)*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))))/(Nz4*den2);
            b45=(ctheta2*(2*Nx*y2*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))+Nx2Nz2*(Ny*(x2-xb)-y2*(Nz+xb*(-y2y1)+x2x1*yb)+x2*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))-Nx2Nz2*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(ctheta2*(xb*(2*Nx*Ny*y2*(-u2*z1+u1*z2)+Nx2*(Ny*u2+u2*x2*z1-u1*x2*z2)+Nz2*(Ny*u2+u2*x2*z1-u1*x2*z2))+(-u2*x1+u1*x2)*(Nx2Nz2*x2-2*Nx*Ny*y2)*zb)-Nz2*(Nx*u2*yb+y2*(Nx*num3)))))/(Nz4*den2);
            b46=(ctheta2*(Nx2Nz2*(Ny*(-x1+xb)+y1*(Nz+xb*(-y2y1)+x2x1*yb)+x1*(xb*(-z2z1)+x1*(z2-zb)+x2*(-z1+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))-2*Nx*y1*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(Nz3*(-u2*xb*y1+u1*xb*y2+u2*x1*yb-u1*x2*yb)+ctheta2*Nx2*Ny*num2+Nz2*(ctheta2*Ny*num2+Nx*num3))+Nx2Nz2*(Nz2+Nz*(xb*(-y2y1)+x2x1*yb)+Nx*(Nz+xb*(-y2y1)+x2x1*yb)+Ny*(xb*z2z1+x2*(z1-zb)+x1*(-z2+zb)))*(-Nz2*(Nx*u1*yb+y1*(Nx*num3))+ctheta2*(2*Nx*Ny*y1*(-u2*xb*z1+u1*xb*z2+u2*x1*zb-u1*x2*zb)+Nx2*(Ny*u1*xb+x1*(num2))+Nz2*(Ny*u1*xb+x1*(num2))))))/(Nz4*den2);
//        }
    }
    
    b81=-zi/R;
    b810=-vbx/R;
    b93=-zi/R;
    b910=-vby/R;
    b101=xi/R;
    b103=yi/R;
    b108=vbx/R;
    b109=vby/R;
    b111=-zk/R;
    b1113=-vbx/R;
    b123=-zk/R;
    b1213=-vby/R;
    b131=xk/R;
    b133=yk/R;
    b1311=vbx/R;
    b1312=vby/R;
    
    
    cvmSet( b, 1, 4, b14 );
    cvmSet( b, 1, 5, b15 );
    cvmSet( b, 1, 6, b16 );
    cvmSet( b, 3, 4, b34 );
    cvmSet( b, 3, 5, b35 );
    cvmSet( b, 3, 6, b36 );
    
    cvmSet( b, 4, 0, b40 );
    cvmSet( b, 4, 2, b42 );
    cvmSet( b, 4, 4, b44 );
    cvmSet( b, 4, 5, b45 );
    cvmSet( b, 4, 6, b46 );
    
    cvmSet( b, 8, 1, b81 );
    cvmSet( b, 8, 10, b810 );
    cvmSet( b, 9, 3, b93 );
    cvmSet( b, 9, 10, b910 );
    cvmSet( b, 10, 1, b101 );
    cvmSet( b, 10, 3, b103 );
    cvmSet( b, 10, 8, b108 );
    cvmSet( b, 10, 9, b109 );
    
    cvmSet( b, 11, 1, b111 );
    cvmSet( b, 11, 13, b1113 );
    cvmSet( b, 12, 3, b123 );
    cvmSet( b, 12, 13, b1213 );
    cvmSet( b, 13, 1, b131 );
    cvmSet( b, 13, 3, b133 );
    cvmSet( b, 13, 11, b1311 );
    cvmSet( b, 13, 12, b1312 );
    
    cvAdd( A, b, *Ab );
    
    cvReleaseMat( &A );
    cvReleaseMat( &b );
    
    return;
}

void fillC( CvMat **C, CvMat **Czkm, CvMat *x_hat){
    
    cvZero(*C);
    
    if (table.cameraHeight>0){        //Recalculate C_ball(x,u)
        double sX = table.cameraHeight/(table.cameraHeight - R/2);
        double sY = table.cameraHeight/(table.cameraHeight - R/2);
        
        cvmSet( *C, 0, 0, sX);
        cvmSet( *C, 1, 2, sY);
    }
    else {
        cvmSet( *C, 0, 0, 1);
        cvmSet( *C, 1, 2, 1);
    }
    
    double xb, yb, z0, z1, z2;
    double xi, yi, zi, xk, yk, zk;
    
    xb=cvmGet( x_hat, 0, 0);
    yb=cvmGet( x_hat, 2, 0);
    z0=cvmGet( x_hat, 4, 0);
    z1=cvmGet( x_hat, 5, 0);
    z2=cvmGet( x_hat, 6, 0);
    xi=cvmGet( x_hat, 8, 0);
    yi=cvmGet( x_hat, 9, 0);
    zi=cvmGet( x_hat, 10, 0);
    xk=cvmGet( x_hat, 11, 0);
    yk=cvmGet( x_hat, 12, 0);
    zk=cvmGet( x_hat, 13, 0);
    
    cvmSet( *C, 2, 4, 1);
    cvmSet( *C, 3, 5, 1);
    cvmSet( *C, 4, 6, 1);
    cvmSet( *C, 5, 8, 1);
    cvmSet( *C, 6, 9, 1);
    cvmSet( *C, 7, 11, 1);
    cvmSet( *C, 8, 12, 1);
    cvmSet( *C, 9, 8, xk); cvmSet( *C, 9, 9, yk); cvmSet( *C, 9, 10, zk);
    cvmSet( *C, 9, 11, xi); cvmSet( *C, 9, 12, yi); cvmSet( *C, 9, 13, zi);
    cvmSet( *C, 10, 11, 2*xk); cvmSet( *C, 10, 12, 2*yk); cvmSet( *C, 10, 13, 2*zk);
    
    cvmSet( *Czkm, 0, 0, xb);
    cvmSet( *Czkm, 1, 0, yb);
    cvmSet( *Czkm, 2, 0, z0);
    cvmSet( *Czkm, 3, 0, z1);
    cvmSet( *Czkm, 4, 0, z2);
    cvmSet( *Czkm, 5, 0, xi);
    cvmSet( *Czkm, 6, 0, yi);
    cvmSet( *Czkm, 7, 0, xk);
    cvmSet( *Czkm, 8, 0, yk);
    cvmSet( *Czkm, 9, 0, xi*xk+yi*yk+zi*zk);
    cvmSet( *Czkm, 10, 0, xk*xk+yk*yk+zk*zk);
    
    return;
}

void kalmanCD_3_predict( CvKalman **kalman_system , CvMat *u){
    
    CvMat *Pkm, *Q_kal, *R_kal;
    
    Q_kal=(*kalman_system)->process_noise_cov;
    R_kal=(*kalman_system)->measurement_noise_cov;
    
//    printf("Entering continuous prediction...\n");
    //===================
    //Continuous prediction
    //===================
    
    CvMat *S, *dSdt, *Sk, *x_hat;
    
    S=cvCreateMat( 16, 16, CV_32F );
    cvCopy((*kalman_system)->error_cov_post, S);
    x_hat=(*kalman_system)->state_post;
//    printCvArr(S);
    dSdt=cvCloneMat( S );
    Sk=cvCloneMat( S );
    
    int i, N=(x_hat)->rows;
    double step=delT, h=step/2.0,                      // the midpoint
            xt0[N], xt1[N], xt2[N], xt3[N],            // temporary storage arrays
            k1[N], k2[N], k3[N], k4[N];      // for Runge-Kutta
    double t=0;
    
    
//    printf("Integrating x...\n");
    //Integrate dxdt
    for (i=0;i<N;i++) xt0[i]=cvmGet(x_hat, i, 0);
    for (i=0;i<N;i++) xt1[i]=cvmGet(x_hat, i, 0)+0.5*(k1[i]=step*dx(t, xt0, u, i));
    for (i=0;i<N;i++) xt2[i]=cvmGet(x_hat, i, 0)+0.5*(k2[i]=step*dx(t+h, xt1, u, i));
    for (i=0;i<N;i++) xt3[i]=cvmGet(x_hat, i, 0)+    (k3[i]=step*dx(t+h, xt2, u, i));
    for (i=0;i<N;i++) k4[i]=                                 step*dx(t+step, xt3, u, i);
    
    for (i=0;i<N;i++) cvmSet((*kalman_system)->state_pre, i, 0, cvmGet(x_hat, i, 0)+(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6.0);
    
    
//    printCvArr(*x_hat);
    
//    printf("Integrating S...\n");
    //Integrate dSdt
    CvMat *tmpS, *St2, *St3, *sk1, *sk2, *sk3, *sk4;
    
    tmpS=cvCloneMat( S );
    St2=cvCloneMat( S );
    St3=cvCloneMat( S );
    sk1=cvCloneMat( S );
    sk2=cvCloneMat( S );
    sk3=cvCloneMat( S );
    sk4=cvCloneMat( S );
    
//    printf("Integrating S1...\n");
//    for (int j=0; j<16; j++){
//        printf("%f, ", xt3[j]);
//    }
    
    dS(S, &dSdt, xt0, u, Q_kal);
//    printf("Done integrating S1...\n");
    cvScale( dSdt, sk1, step);
    cvScaleAdd(sk1, cvScalar(0.5), S, tmpS);
    
//    printf("Integrating S2...\n");
    dS(tmpS, &dSdt, xt1, u, Q_kal);
    cvScale( dSdt, sk2, step );
    cvScaleAdd(sk2, cvScalar(0.5), S, St2);
    
//    printf("Integrating S3...\n");
    dS(St2, &dSdt, xt2, u, Q_kal);
    cvScale( dSdt, sk3, step);
    cvAdd( sk3, S, St3 );
    
//    printf("Integrating S4...\n");
    dS(St3, &dSdt, xt3, u, Q_kal);
    cvScale( dSdt, sk4, step);
    
//    printf("Assembling integration steps...\n");
    cvScaleAdd( sk2, cvScalar(2), sk1, tmpS );
    cvScaleAdd( sk3, cvScalar(2), tmpS, tmpS );
    cvAdd( sk4, tmpS, tmpS );
    cvScaleAdd( tmpS, cvScalar(1/6.0), S, Sk );
    
    cvTranspose(Sk, S);
    cvAdd(Sk, S, Sk);
    cvScale( Sk, (*kalman_system)->error_cov_pre, 0.5 );
        
//    printf("\n Sk- = \n");
//    printCvArr( (*kalman_system)->error_cov_pre );
//    
//    printf("\n x_hat = \n");
//    printCvArr( (*kalman_system)->state_pre );
    
    // Local garbage collection
    cvReleaseMat( &tmpS );
    cvReleaseMat( &St2 );
    cvReleaseMat( &St3 );
    cvReleaseMat( &sk1 );
    cvReleaseMat( &sk2 );
    cvReleaseMat( &sk3 );
    cvReleaseMat( &sk4 );
    cvReleaseMat( &dSdt );
    cvReleaseMat( &S );
    cvReleaseMat( &Sk );
}

void kalmanCD_3_correct( CvKalman **kalman_system , CvMat *y_measrdR){
  
    
//    printf("Entering discrete prediction...\n");
    //===================
    //Discrete correction
    //===================
    
    CvMat *x_hat;
    CvMat *C, *Czkm;
    CvMat *S, *Sk, *SkInv;
    CvMat *R_kal, *RInv, *CtRInv;
    CvMat *predErr, *kalCorr;
    
    x_hat=(*kalman_system)->state_pre;
    Sk=(*kalman_system)->error_cov_pre;
    R_kal=(*kalman_system)->measurement_noise_cov;
    C=(*kalman_system)->measurement_matrix;
    
    Czkm=cvCloneMat(y_measrdR);
    S=cvCloneMat( Sk );
    SkInv=cvCloneMat( Sk );
    RInv=cvCloneMat( R_kal );
    kalCorr=cvCreateMat( 16, 11, CV_32F );
    CtRInv=cvCloneMat(kalCorr);
    predErr=cvCloneMat(y_measrdR);
    
//    printf("Forming C...\n");
    
    fillC( &C, &Czkm, x_hat );
    
//    printf("\n x_hat = \n");
//    printCvArr( x_hat );
//    
//    printf("\n y_measrdR = \n");
//    printCvArr( y_measrdR);
//    
//    printf("\n Czkm = \n");
//    printCvArr( Czkm);
//    
//    printf("\n C = \n");
//    printCvArr( C);
//    
//    
//    printf("\n Sk- = \n");
//    printCvArr( Sk );
//    
//    printf("\n Q_kal = \n");
//    printCvArr( (*kalman_system)->process_noise_cov );
//    
//    printf("\n R_kal = \n");
//    printCvArr( R_kal );
    
//    printf("Inverting R...\n");
    cvInvert( R_kal, RInv, CV_SVD_SYM );
    
    cvGEMM(C, RInv, 1, 0, 0, CtRInv, CV_GEMM_A_T);
    cvGEMM(CtRInv, C, delT, Sk, 1, Sk, 0);
    cvTranspose(Sk, S);
    cvAdd(Sk, S, Sk);
    cvScale( Sk, (*kalman_system)->error_cov_post, 0.5 );
    
    cvInvert( (*kalman_system)->error_cov_post, SkInv, CV_SVD_SYM );    
    cvScaleAdd( y_measrdR, cvScalar(-1), Czkm, predErr);
    
    cvGEMM( SkInv, CtRInv, delT, 0, 0, kalCorr, 0);
    cvGEMM( kalCorr, predErr, -1, x_hat, 1, (*kalman_system)->state_post, 0 );
    
    
    double xi, yi, zi, xk, yk, zk;
    
    xi=cvmGet( (*kalman_system)->state_post, 8, 0);
    yi=cvmGet( (*kalman_system)->state_post, 9, 0);
    zi=cvmGet( (*kalman_system)->state_post, 10, 0);
    xk=cvmGet( (*kalman_system)->state_post, 11, 0);
    yk=cvmGet( (*kalman_system)->state_post, 12, 0);
    zk=cvmGet( (*kalman_system)->state_post, 13, 0);
    cvmSet( (*kalman_system)->state_post, 14, 0, xi*xk+yi*yk+zi*zk);
    cvmSet( (*kalman_system)->state_post, 15, 0, xk*xk+yk*yk+zk*zk);

//    printf("\n SkInv+ = \n");
//    printCvArr( SkInv );
//    
//    printf("\n CtRInv+ = \n");
//    printCvArr( CtRInv );
//    
//    printf("\n kalCorr = \n");
//    printCvArr( kalCorr );
//    printf("\n Sk-+ = \n");
//    printCvArr( Sk );
//    
////    Add the transpose and take the average in order to ensure symmetry
//
//    
//    printf("\n Sk+ = \n");
//    printCvArr( (*kalman_system)->error_cov_post );
//    
//    printf("\n x_hat = \n");
//    printCvArr( (*kalman_system)->state_post );
//    printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
////    exit(1);
    
    // Garbage collection
    cvReleaseMat( &S );
    cvReleaseMat( &SkInv );
    cvReleaseMat( &RInv );
    cvReleaseMat( &Czkm );
    cvReleaseMat( &CtRInv );
    cvReleaseMat( &predErr );
    cvReleaseMat( &kalCorr );

    return;
}
